#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   dual_arm_manager.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.1
@Create Time    :   CURRENT_YEAR/CURRENT_MONTH/CURRENT_DATE CURRENT_HOUR:CURRENT_MINUTE:CURRENT_SECOND
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   None 
'''

# here put the import lib


# the import lib
import logging
import threading
import subprocess

import uservo
import serial
import time

from MotionGroup import MotionGroup
from ServoPose import Pose

from wrist_gripper import WristGripperControl
from robot_arm import ArmMotionControl

import rospy

# flag
exitFlag = 0  # threading control flag
VERBOSE = False #rospr loginfo flag
G_CONTROL_MODE = 3  # control mode server 0,1,2,3 default is landing mode

HOST_STATE = True # the homer host ip port state , default is true

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 1.0

# publisher
SAMPLING_FREQUENCY = 10  # units: Hz
fileName = "robot_arm_records.txt"
POSE_NUM = rospy.get_param('/teach_mode/way_point_num')  # default pose number is 300


ending_servo_idx = [0, 1, 2, 3]

# dual_arm class obj init
dual_arm = ArmMotionControl(arm_name="dual_arm", 
                            dof=rospy.get_param('/arm_info/dual_arm_dof'), 
                            CONTROL_MODE="DAMPING", 
                            with_wrist=False, 
                            is_debug=True,
                            damping_power=1000)
wrist_gripper = WristGripperControl(servo_channel=ending_servo_idx, is_debug=True)

# 初始化舵机管理器类对象、串口端口地址
dual_arm_obj = dual_arm.creat_uservo_obj(SERVO_BAUDRATE=115200)
if dual_arm.servo_buffer(is_scan_servo=True):
    # dual arm and gripper init
    dual_arm.arm_state_init(force_angle_init=False)
    dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper.all_ending_init(motion_group=4)
    rospy.loginfo("-----dual_arm init success!-----")
    dual_arm.logging_arm_info()         # logging servo info 
    dual_arm.get_arm_current_angle()    # logging servo angle info
    time.sleep(1)
else:
    rospy.logerr("ERROR: dual arm and gripper init failed!!!")
    


# if __name__ == '__main__':
#     try:
#         main()
#     except KeyboardInterrupt:
#         rospy.logerr("dual_arm_manager exit()")



